#ifndef __SENSOE_LAYER_H__
#define __SENSOE_LAYER_H__

#include "../common/costmap_base.h"

#include <geometry_msgs/TransformStamped.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/PointCloud2.h>
#include <laser_geometry/laser_geometry.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <deque>

namespace costmap_2d{

struct SensorData{
    geometry_msgs::TransformStamped sensor_pose;
    sensor_msgs::PointCloud2 cloud;
};

class SensorLayer{
    public:
        SensorLayer(boost::shared_ptr<Costmap> realtime_costmap_ptr, tf2_ros::Buffer* tf_buffer);
        ~SensorLayer();

        // 添加二维激光雷达传感器数据
        void addLidar2DScanData(sensor_msgs::LaserScan::Ptr msg_ptr);
        // 添加深度点云数据
        void addPointCloud2Data(sensor_msgs::PointCloud2::Ptr msg_ptr);

        // 传感器队列裁剪
        void purgeDeque();

        // 更新代价地图
        void updateCostmap(bool is_local);
        // 清空代价地图
        void clearCostmap();

        // 返回障碍物数据
        std::unordered_map<int, GridCell> getObstacleCells(){
            return obstacleCells_;
        }

    private:
        ros::NodeHandle nh_;
        ros::Publisher debugOriginPointCloudPub_;
        ros::Publisher debugTransformedPointCloudPub_;
        
        tf2_ros::Buffer* const tfBuffer_;
        laser_geometry::LaserProjection projector_;
        boost::shared_ptr<Costmap> originCostmapPtr_;       //全局静态地图
        boost::shared_ptr<Costmap> realtimeCostmapPtr_;   //实时代价地图
        std::deque<SensorData> pointCloudDeque_;     //传感器队列
        ros::Duration keepTime_;       //传感器数据保留时间
        std::unordered_map<int, GridCell> obstacleCells_;
};

}; //costmap_2d

#endif